#include <xdc/std.h>
#include <xdc/runtime/System.h>
#include <stdio.h>

#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Mailbox.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Task.h>

#include <ti/sysbios/hal/Hwi.h>
#include <ti/sysbios/hal/Timer.h>

#include <inc/hw_sysctl.h>
#include <inc/hw_memmap.h>
#include <inc/hw_types.h>
#include <linemouse_chassis.h>
#include <ti/drivers/GPIO.h>

#include "../TskTop/DbgUart.h"
#include "../TskRun/TskRun.h"
#include "../TskMotor/TskMotor.h"
#include "../Queue/Queue.h"



Semaphore_Handle SemRunTick;

namespace TskRun
{
	const int tskPrio = 11;
	const int tskStkSize = 2048;
	Task_Handle tsk;



	void task(UArg arg0, UArg arg1)
	{


	    static int s_cnt = 0;
	    TskMotor::VelOmega desire(0, 0);// desired lv & av to queue

		while(true)
		{

	        if(!Semaphore_pend(SemRunTick, 2))
	            System_abort("pend SemRunTick failed!\n");

            if (s_cnt>=1000 && s_cnt<=1500)
            {
            	desire.Velocity = 80;
				desire.Omega = 0;


				TskMotor::QMotor->En(desire);

            }
            else
            {
            	desire.Velocity = 0;
				desire.Omega = 0;


				TskMotor::QMotor->En(desire);
            }


            s_cnt++;
		}
	}

	void Init()
	{
		Task_Params tskParams;
		Semaphore_Params semParams;

		Task_Params_init(&tskParams);
		tskParams.priority = tskPrio;
		tskParams.stackSize = tskStkSize;
		tsk = Task_create(task, &tskParams, NULL);
		if(tsk == NULL)
			System_abort("TskRun failed.");

	    Semaphore_Params_init(&semParams);
	    semParams.mode = Semaphore_Mode_BINARY;
	    SemRunTick = Semaphore_create(0, &semParams, NULL);
	    if(SemRunTick == NULL)
	        System_abort("create SemRunTick failed.\n");

	}


}
